package com.calmcar.adas.util;

import com.calmcar.adas.apiserver.AdasConf;
import com.calmcar.adas.apiserver.model.AdasRect;
import com.calmcar.adas.apiserver.model.CdwDetectInfo;
import com.calmcar.adas.apiserver.model.FrontCarInfo;
import com.calmcar.adas.apiserver.model.LdwDetectInfo;
import com.calmcar.adas.conf.Constant;

import org.opencv.core.Point;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

/**
 * Created by lelexiao on 2018/4/8.
 */

public class FrontCarLaunchUtil {
    static List<Double> queue = new ArrayList<>();
    static double front_y = 0;

    public static boolean checkIsFrontCarLunch(double x, double y, double rate) {
        if (queue.size() < 3) {
            queue.add(y);
            front_y = 0;
        } else {
            front_y = (queue.get(0) + queue.get(1) + queue.get(2)) / 3;
            if (y + 10 < front_y) {
                return true;
            }
        }
        return false;
    }


    public static void clearPointList() {
        queue.clear();
    }




    public static AdasRect checkIsFrontCarForAPIHalf(CdwDetectInfo cdwDetectInfo, LdwDetectInfo ldwDetectInfo) {
        List<AdasRect> curList = cdwDetectInfo.carRects;
        if (curList == null || curList.size() == 0) return null;
        List<Integer> listIndex = new ArrayList<>();
        List<Double> listRectValue = new ArrayList<>();
        for (int m = 0; m < curList.size(); m++) {
            AdasRect curCar = curList.get(m);
            if (FrontCarLaunchUtil.checkIsInAreaNewForAPIHalf(curCar, ldwDetectInfo)) {
                listIndex.add(m);
                listRectValue.add(curCar.getBr().getX() - curCar.getT1().getX());
            }
        }

        if (listIndex.size() <= 0) {
            return null;
        } else if (listIndex.size() == 1) {
            return curList.get(listIndex.get(0));
        } else {
            return curList.get(listIndex.get(listRectValue.indexOf(Collections.max(listRectValue))));
        }
    }




    public static boolean checkIsInAreaNewForAPIHalf(AdasRect pointNew, LdwDetectInfo ldwDetectInfo) {
        double pointBottomCenterX = (pointNew.getT1().getX() + pointNew.getBr().getX()) / 2;
        double centX;
        if(AdasConf.VP_X>0){
            centX = AdasConf.VP_X;
        }else{
            centX = AdasConf.centerX;
        }
        if(pointBottomCenterX > centX-120 &&  pointBottomCenterX < centX+120 ){
            return true;
        }
        return false;

    }

    public static AdasRect checkIsFrontCarForAPI(CdwDetectInfo cdwDetectInfo, LdwDetectInfo ldwDetectInfo) {
        List<AdasRect> curList = cdwDetectInfo.carRects;
        if (curList == null || curList.size() == 0) return null;
        List<Integer> listIndex = new ArrayList<>();
        List<Double> listRectValue = new ArrayList<>();
        for (int m = 0; m < curList.size(); m++) {
            AdasRect curCar = curList.get(m);
            if (FrontCarLaunchUtil.checkIsInAreaNewForAPI(curCar, ldwDetectInfo)) {
                listIndex.add(m);
                listRectValue.add(curCar.getBr().getX() - curCar.getT1().getX());
            }
        }

        if (listIndex.size() <= 0) {
            return null;
        } else if (listIndex.size() == 1) {
            return curList.get(listIndex.get(0));
        } else {
            return curList.get(listIndex.get(listRectValue.indexOf(Collections.max(listRectValue))));
        }
    }


    public static boolean checkIsInAreaNewForAPI(AdasRect pointNew, LdwDetectInfo ldwDetectInfo) {
        Point pointBottomCenter = new Point((pointNew.getT1().getX() + pointNew.getBr().getX()) / 2, pointNew.getBr().getY());
        List<Point> APoints = new ArrayList<>();
        double centX;
        double centY;
        if(AdasConf.VP_X>0){
           centX = AdasConf.VP_X;
             centY = AdasConf.VP_Y;
        }else{
               centX = AdasConf.centerX;
               centY = AdasConf.centerY;
        }
        Point point0 = new Point(centX - 20, centY - 10);
        Point point1 = new Point(centX - 30, centY + 80);
        Point point2 = new Point(centX - 50, centY + 400);
        Point point3 = new Point(centX + 50, centY + 400);
        Point point4 = new Point(centX + 30, centY + 80);
        Point point5 = new Point(centX + 20, centY - 10);
        APoints.add(point0);
        APoints.add(point1);
        APoints.add(point2);
        APoints.add(point3);
        APoints.add(point4);
        APoints.add(point5);
        if (ldwDetectInfo != null && ldwDetectInfo.getLdwType() == 1) {
            APoints.add(new Point(ldwDetectInfo.lineRect[0].getX(), ldwDetectInfo.lineRect[0].getY() - 50));
            APoints.add(ldwDetectInfo.lineRect[1].convertPoint());
            APoints.add(ldwDetectInfo.lineRect[2].convertPoint());
            APoints.add(ldwDetectInfo.lineRect[3].convertPoint());
            APoints.add(ldwDetectInfo.lineRect[4].convertPoint());
            APoints.add(new Point(ldwDetectInfo.lineRect[5].getX(), ldwDetectInfo.lineRect[0].getY() - 50));
        }
        int nCross = 0;
        for (int i = 0; i < APoints.size(); i++) {
            Point p1 = APoints.get(i);
            Point p2 = APoints.get((i + 1) % APoints.size());
            // 求解 y=p.y 与 p1p2 的交点
            if (p1.x == p2.x)      // p1p2 与 y=p0.y平行
                continue;
            if (pointBottomCenter.x < Math.min(p1.x, p2.x))   // 交点在p1p2延长线上
                continue;
            if (pointBottomCenter.x >= Math.max(p1.x, p2.x))   // 交点在p1p2延长线上
                continue;
            // 求交点的 X 坐标 --------------------------------------------------------------
            double x = (double) (pointBottomCenter.x - p1.x) * (double) (p2.y - p1.y) / (double) (p2.x - p1.x) + p1.y;
            if (x > pointBottomCenter.y)
                nCross++; // 只统计单边交点
        }
        // 单边交点为偶数，点在多边形之外 ---
        return (nCross % 2 == 1);

    }

    private List<Double> headBaseList;
    private double headBaseValue = -1;
    private int checkLaunchTimes = 0;

    public boolean isInit() {
        return isInit;
    }

    public void setInit(boolean init) {
        isInit = init;
    }

    private  boolean isInit=true;
    private  int carNullTick=0;
    public FrontCarLaunchUtil() {
        headBaseList = new ArrayList();
    }
    private double yOld=1000;
    public boolean checkFrontCarLaunchNew(double y) {
        setStillState();
        if (headBaseList.size() < 1) {
            headBaseList.add(y);
        } else {
            if (headBaseValue < 0) {
                headBaseValue = headBaseList.get(0) ;
            }
        }
        //逐渐缩小
        if (y + AdasConf.CAR_LAUNCH_DIS< headBaseValue  && y < yOld ) {
            yOld = y;
            checkLaunchTimes += 1;
        }
        if (checkLaunchTimes >= AdasConf.CHECK_TIMES) {
            return true;
        }
        return false;
    }


    public boolean checkFrontCarLaunchNew(FrontCarInfo frontCarInfo ) {
        setStillState();
        double adbDis=frontCarInfo.getCarRect().getAbsDis();
        if(yABSOld==0){
            yABSOld=adbDis;
            return false;
        }
        if (adbDis> 3-AdasConf.FCW_LAUNCH_LEVEL*0.8 && adbDis> yABSOld +1.1 && adbDis < 15) {
            checkLaunchTimes += 1;
        }

        if (checkLaunchTimes >= AdasConf.CHECK_ABS_TIMES) {
            isInit=true;
            return true;
        }
        return false;
    }

    private double yABSOld=0;
    public boolean checkFrontCarABSLaunchNew(double y) {
        setStillState();
        if (headBaseList.size() < 1) {
            headBaseList.add(y);
            return false;
        } else {
            if (headBaseValue < 0) {
                headBaseValue = headBaseList.get(0) ;
            }
        }

        if (y > headBaseValue+(AdasConf.CAR_LAUNCH_ABS_DIS+AdasConf.FCW_LAUNCH_LEVEL*0.3)  && y > yABSOld ) {
            yABSOld= y;
            checkLaunchTimes += 1;
        }
        if (checkLaunchTimes >= AdasConf.CHECK_ABS_TIMES) {
            return true;
        }
        return false;
    }



    public   void setStillState() {
        if(isInit){
            isInit=false;
            headBaseList.clear();
            headBaseValue = -1;
            checkLaunchTimes = 0;
            yOld=1000;
            yABSOld=0;
            carNullTick=0;
        }
    }
}
